/*
 * COPYRIGHT (C) 2005-2010
 * RE2, INC.
 * ALL RIGHTS RESERVED
 *
 *
 * THIS WORK CONTAINS VALUABLE CONFIDENTIAL AND PROPRIETARY INFORMATION.
 * DISCLOSURE OR REPRODUCTION WITHOUT THE WRITTEN AUTHORIZATION OF RE2, INC.
 * IS PROHIBITED. THIS UNPUBLISHED WORK BY RE2, INC. IS PROTECTED BY THE LAWS
 * OF THE UNITED STATES AND OTHER COUNTRIES. IF PUBLICATION OF THE WORK SHOULD
 * OCCUR, THE FOLLOWING NOTICE SHALL APPLY.
 *
 * "COPYRIGHT (C) 2005-2010 RE2, INC. ALL RIGHTS RESERVED."
 *
 * RE2, INC. DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE, INCLUDING
 * ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO EVENT SHALL
 * RE2, INC. BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR
 * ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER
 * IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT
 * OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */

#ifndef STATEPUBLISHER_H_
#define STATEPUBLISHER_H_
#include <robot_state_publisher/robot_state_publisher.h>
#include <urdf/model.h>
#include <ros/ros.h>
#include <WAM_msgs/WAMEnums.h>
#include <WAM_msgs/ArmPositions.h>
#include <DPPanTilt_msgs/ReportDynamicInfo.h>
#include <BHand_msgs/BHandEnums.h>
#include <BHand_msgs/BHandPositions.h>


#define KNUCKLEONE      0
#define KNUCKLETWO      1
#define KNUCKLETHREE    2
#define KNUCKLE_DOF     3


class StatePublisher
{
public:
	StatePublisher(const ros::NodeHandle& nodeHandle,const std::string& urdfData);
	virtual ~StatePublisher();
	void start();
	void stop();
	
private:
	ros::NodeHandle m_nodeHandle;
	ros::Subscriber m_leftWamPositionsSubscriber;
    ros::Subscriber m_rightWamPositionsSubscriber;
	ros::Subscriber m_lowerPTUSubscriber;
	ros::Subscriber m_upperPTUSubscriber;
    ros::Subscriber m_leftBHandSubscriber;
    ros::Subscriber m_rightBHandSubscriber;
    ros::Subscriber m_rightHandSub;
    ros::Subscriber m_leftHandSub;

	std::map<std::string, double>* m_jointMap;

	bool m_isRunning;

	std::string m_leftWamJointNames[WAM_DOF];
    std::string m_rightWamJointNames[WAM_DOF];
    std::string m_leftBHandJointNames[3][3];
    std::string m_rightBHandJointNames[3][3];
    //std::string m_lowerPanTiltJointNames[4];
    //std::string m_upperPanTiltJointNames[2];

	robot_state_publisher::RobotStatePublisher* m_publisher;

	void leftWamPositionsCallback(const WAM_msgs::ArmPositionsConstPtr& msg);
    void rightWamPositionsCallback(const WAM_msgs::ArmPositionsConstPtr& msg);
	void upperPTUCallback(const DPPanTilt_msgs::ReportDynamicInfoConstPtr& msg);
	void lowerPTUCallback(const DPPanTilt_msgs::ReportDynamicInfoConstPtr& msg);
	void rightHandPositionsCallback(const BHand_msgs::BHandPositionsConstPtr& msg);
	void leftHandPositionsCallback(const BHand_msgs::BHandPositionsConstPtr& msg);

	void loop();
};

#endif /* STATEPUBLISHER_H_ */
